function [TrueParameters EstmParameters] = Bug2()
dbstop if error
pb=projBug();

t = 0;
dt = 0.1;
T = 10;

Ncam = 3;

% the true parameters defining the motion
TrueParameters = [];
% our running estimates
EstmParameters = [];

while( t < T )
    t = t + dt;

    % make the angles depend on time.
    % from only the projected views, try to invert the operation
    % and figure out what the joint positions were
    
    [lpts0 xhat] = pb.getPointCloud( t );

    TrueParameters = [ TrueParameters xhat ]; % store the true joint angles

    if( t == dt ) % the initial position in state space is given
        EstmParameters = TrueParameters;
    end





    % generate N views of the target

    figure(1);
    allPts = cell(1,Ncam);
    allCam = cell(1,Ncam);
    for k = 1:Ncam
        posk = rz( 2*pi*k/Ncam ) * [40 40 40]';
        camA = GetCameraS03Matrix( posk ); % the matrix that transforms the camera to be at pos1, viewing the origin

        ptsCam = camA * [lpts0; ones(1,size(lpts0,2))];
        PI = [eye(3,3) zeros(3,1)];
        lptsImg = PI*ptsCam;
        ptsX = lptsImg(1,:)./lptsImg(3,:);
        ptsY = lptsImg(2,:)./lptsImg(3,:);

        plot(ptsX,ptsY,'.r'); axis([-0.5 0.5 -0.5 0.5]);
        allPts{k} = [ptsX(:) ptsY(:)];
        allCam{k} = camA;
        drawnow; % draw now 
    end;

    if( t > dt ) % estimate current state from current images, camera calibration data,
        % the bug model, the all previous state estimates
        curEst = EstimateState( allPts, allCam, pb, EstmParameters, TrueParameters );

        EstmParameters = [EstmParameters curEst];
    end;

    drawnow;
    t
    breakhere = 1;


  end

end

% helper functions
function re=rx(alpha)
re=[1 0 0; 0 cos(alpha) -sin(alpha) ; 0 sin(alpha) cos(alpha)];
end
function re=ry(alpha)
re=[cos(alpha) 0 sin(alpha); 0 1 0  ; -sin(alpha) 0 cos(alpha)];
end
function re=rz(alpha1)
re=[ cos(alpha1) -sin(alpha1) 0; sin(alpha1) cos(alpha1) 0; 0 0 1];
end
function h=adjoint(g0,g1)
 h=inver(g0)*g1*g0;
end
function t=inver(g)
r=(g((1:3),(1:3)))^-1;
d=-((g((1:3),(1:3)))^-1)*g((1:3),4);
t=[r d; 0 0 0 1];
end